/*
 * Kruispunt_AVR.c
 *
 * Created: 15-5-2013 9:58:12
 *  Author: Matthijs
 */


#include "Kruispunt AVR.h"
#include "I2C.h"

int main(void)
{	
	init();
    while(1)
    {
	    while((TWSR & 0xF8)!= 0x60)  // Loop till correct acknoledgement have been received
	    {
		    // Get acknowlegement, Enable TWI, Clear TWI interrupt flag
		    TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
		    while (!(TWCR & (1<<TWINT)))  // Wait for TWINT flag
		    {
				Get_Sensors();
				set_State();
				set_Traficlight();
				set_delay();
			}
		}
		parse_data_crossroad();	
    }
}

void init(void)
{
	DDRC = (1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4)
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0);

	PORTC &= ~((1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4)
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0));
	DDRA = 0xC0;
	DDRB = 0x0F;
	DDRD = 0xFC;
	PORTB = 0; //verkeerslichten gaan een kaar aan/uit
	TWI_init_slave(); // Function to initilaize slave
	_mode = 1;
	presentState = 1;
	_delay_ms(1000);
	PORTB = ~0;
}



void Get_Sensors()
{
	if ((PINA & (1<<0)) == 0)
	{
		senArray[0] = 1;
	}
	else
	{
		senArray[0]= 0;
	}

	if ((PINA & (1<<1)) == 0)
	{
		senArray[1] = 1;
	}
	else
	{
		senArray[1]=0;
	}

	if ((PINA & (1<<2)) == 0)
	{
		senArray[2] = 1;
	}
	else
	{
		senArray[2]=0;
	}
	if ((PINA & (1<<3)) == 0)
	{
		senArray[3] = 1;
	}
	else
	{
		senArray[3] = 0;
	}

	if ((PINA & (1<<4)) == 0)
	{
		senArray[4] = 1;
	}
	else
	{
		senArray[4] = 0;
	}

	if ((PINA & (1<<5)) == 0)
	{
		senArray[5] = 1;
	}
	else
	{
		senArray[5] = 0;
	}
}

void set_State()
{
	int Tempstate = 0;
	//Process
	if(presentState==1)//Gedaan
	{
		Tempstate = 1;
		int staArray[10][7] = //staArray is een template, als de eerste 6 waardes overeenkomen met de sensor ga naar state [positie 7]
		{
			{0,1,0,0,1,0,2},
			{0,0,1,0,0,1,4},
			{1,0,0,0,0,0,6},
			{0,0,0,1,0,0,8},
			{0,1,1,0,0,0,10},
			{0,0,0,0,1,1,12},
			{0,1,0,0,0,0,25},
			{0,0,1,0,0,0,26},
			{0,0,0,0,1,0,27},
			{0,0,0,0,0,1,28}
		};
		for(int i = 0; i<10; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==2)//Gedaan
	{
		Tempstate = 3;
		int staArray[5][7] =
		{
			{0,1,0,0,0,0,14},
			{0,0,0,0,1,0,17},
			{0,1,0,0,1,0,2},
			{0,1,0,0,0,0,30},
			{0,0,0,0,1,0,35}
		};
		for(int i = 0; i<5; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==3)//Gedaan
	{
		Tempstate = 1;
	}
	else if(presentState==4)//Gedaan
	{
		Tempstate = 5;
		int staArray[5][7] =
		{
			{0,0,1,0,0,0,31},
			{0,0,0,0,1,1,29},
			{0,1,1,0,0,0,20},
			{0,0,1,0,0,1,4},
			{0,0,0,0,0,1,34}
		};
		for(int i = 0; i<5; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==5)//Gedaan
	{
		Tempstate=1;
	}
	else if(presentState==6)//Gedaan
	{
		Tempstate = 7;
		int staArray[1][7] =
		{
			{1,0,0,0,0,0,6},
		};
		for(int i = 0; i<1; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==7)//Gedaan
	{
		Tempstate=1;
	}
	else if(presentState==8)//Gedaan
	{
		Tempstate = 9;
		int staArray[1][7] =
		{
			{0,0,0,4,0,0,8}
		};
		for(int i = 0; i<1; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==9)//Gedaan
	{
		Tempstate=1;
	}
	else if(presentState==10)//Gedaan
	{
		Tempstate = 11;
		int staArray[5][7] =
		{
			{0,0,1,0,0,1,19},
			{0,1,0,0,1,0,15},
			{0,0,1,0,0,0,33},
			{0,1,0,0,0,0,36},
			{0,1,1,0,0,0,10},
		};
		for(int i = 0; i<5; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==11)//Gedaan
	{
		Tempstate=1;
	}
	else if(presentState==12)//Gedaan
	{
		Tempstate = 13;
		int staArray[5][7] =
		{
			{0,1,0,0,1,0,16},
			{0,0,1,0,0,1,18},
			{0,0,0,0,0,1,37},
			{0,0,0,0,1,0,32},
			{0,0,0,0,1,1,12},
		};
		for(int i = 0; i<5; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==13)//Gedaan
	{
		Tempstate=1;
	}
	else if(presentState==14)//Gedaan
	{
		Tempstate = 10;
	}
	else if(presentState==15)//Gedaan
	{
		Tempstate = 2;
	}
	else if(presentState==16)//Gedaan
	{
		Tempstate = 2;
	}
	else if(presentState==17)//Gedaan
	{
		Tempstate=12;
	}
	else if(presentState==18)//Gedaan
	{
		Tempstate = 4;
	}
	else if(presentState==19)//Gedaan
	{
		Tempstate = 4;
	}
	else if(presentState==20)//Gedaan
	{
		Tempstate = 10;
	}
	else if(presentState==21)//Gedaan
	{
		Tempstate = 1;
	}
	else if(presentState==22)//Gedaan
	{
		Tempstate = 1;
	}
	else if(presentState==23)//Gedaan
	{
		Tempstate = 1;
	}
	else if(presentState==24)//Gedaan
	{
		Tempstate = 1;
	}
	else if(presentState==25)//Gedaan
	{
		Tempstate = 21;
		int staArray[3][7] =
		{
			{0,1,0,0,0,0,25},
			{0,1,0,0,1,0,2},
			{0,1,1,0,0,0,10}
		};
		for(int i = 0; i<3; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==26)//Gedaan
	{
		Tempstate = 22;
		int staArray[3][7] =
		{
			{0,0,1,0,0,0,26},
			{0,1,1,0,0,0,10},
			{0,0,1,0,0,1,4}
		};
		for(int i = 0; i<3; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==27)//Gedaan
	{
		Tempstate = 23;
		int staArray[3][7] =
		{
			{0,0,0,0,1,0,27},
			{0,1,0,0,1,0,2},
			{0,0,0,0,1,1,12}
		};
		for(int i = 0; i<3; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==28)//Gedaan
	{
		Tempstate = 24;
		int staArray[3][7] =
		{
			{0,0,0,0,0,1,28},
			{0,0,0,0,1,1,12},
			{0,0,1,0,0,1,4}
		};
		for(int i = 0; i<3; i++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<6; tellerTwee++)
			{
				if(senArray[tellerTwee]==staArray[i][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==6)
					{
						Tempstate=staArray[i][6];
					}
				}
			}
		}
	}
	else if(presentState==29)//Gedaan
	{
		Tempstate = 12;
	}
	else if(presentState==30)//Gedaan
	{
		Tempstate = 25;
	}
	else if(presentState==31)//Gedaan
	{
		Tempstate = 26;
	}
	else if(presentState==32)//Gedaan
	{
		Tempstate = 27;
	}
	else if(presentState==33)//Gedaan
	{
		Tempstate = 26;
	}
	else if(presentState==34)//Gedaan
	{
		Tempstate = 28;
	}
	else if(presentState==35)//Gedaan
	{
		Tempstate = 27;
	}
	else if(presentState==36)//Gedaan
	{
		Tempstate = 25;
	}
	else if(presentState==37)//Gedaan
	{
		Tempstate = 28; //volgens mij moet dit 27 zijn
	}
	if(Tempstate != 0)
		presentState = Tempstate;
	for(int i = 0; i<6; i++)
	{
		if(_mode != 1)
			stoplichtArray[i] = _crossroad[presentState-1][i];
		else
			stoplichtArray[i] = _default[presentState-1][i];
	}
}

void set_Traficlight()
{

	orByteA = 0b00000000;
	orByteB = 0b00000000;
	orByteC = 0b00000000;
	orByteD = 0b00000000;
	//---------------------------------------------------------------------------------
	
	if(stoplichtArray[0] == 1)//klopt
	{
		orByteB |= 0b00000010;
	}
	else if(stoplichtArray[0] == 2)//klopt
	{
		orByteB |= 0b00000100;
	}
	else if(stoplichtArray[0] == 3)//klopt
	{
		orByteB |= 0b00001000;
	}
	//------------------------------------
	if(stoplichtArray[1] == 1)//klopt
	{
		orByteA |= 0b10000000;
	}
	else if(stoplichtArray[1] == 2)//klopt
	{
		orByteA |= 0b01000000;
	}
	else if(stoplichtArray[1] == 3)//klopt
	{
		orByteB |= 0b00000001;
	}
	//------------------------------------
	if(stoplichtArray[2] == 1)//klopt
	{
		orByteC |= 0b00010000;
	}
	else if(stoplichtArray[2] == 2)//klopt
	{
		orByteC |= 0b00001000;
	}
	else if(stoplichtArray[2] == 3)//klopt
	{
		orByteC |= 0b00000100;
	}
	//------------------------------------
	if(stoplichtArray[3] == 1)//klopt
	{
		orByteC |= 0b10000000;
	}
	else if(stoplichtArray[3] == 2)//klopt
	{
		orByteC |= 0b01000000;
	}
	else if(stoplichtArray[3] == 3)//klopt
	{
		orByteC |= 0b00100000;
	}
	//------------------------------------
	if(stoplichtArray[4] == 1)//klopt
	{
		orByteD |= 0b00100000;
	}
	else if(stoplichtArray[4] == 2)//klopt
	{
		orByteD |= 0b01000000;
	}
	else if(stoplichtArray[4] == 3)//klopt
	{
		orByteD |= 0b10000000;
	}
	//------------------------------------
	if(stoplichtArray[5] == 1)//klopt
	{
		orByteD |= 0b00000100;
	}
	else if(stoplichtArray[5] == 2)//klopt
	{
		orByteD |= 0b00001000;
	}
	else if(stoplichtArray[5] == 3)//klopt
	{
		orByteD |= 0b00010000;
	}
	
	PORTA = orByteA;
	PORTB = orByteB;
	PORTC = orByteC;
	PORTD = orByteD;
}

void set_delay()
{
	if(orByteB == 0b00001000 || orByteB == 0b00000011 || orByteC == 0b10000100 || orByteC == 0b00110000 || orByteD == 0b10000100 || orByteD == 0b00010100 || (orByteB == 0b00000011 && orByteD == 0b10000100) || (orByteC == 0b00000100 && orByteD == 0b00100100) || (orByteB == 0b00000010 && orByteA == 0b10000000) || orByteD == 0b00100100)
	{
		_delay_ms(1000);
	}
	else if(orByteB == 0b00000100 || orByteA == 0b10000000 || orByteC == 0b10001000 || orByteC == 0b01010000 || orByteD == 0b01000100 || orByteD == 0b01001000 )
	{
		_delay_ms(500);
	}
	else
	{
		_delay_ms(100);
	}
}
